1. Field of the Invention
The subject invention relates to a method for controlling a robot such that a tool attached to the robot may follow a Cartesian path without excessive joint speed while encountering a singularity.
2. Description of the Related Art
An industrial robot can be viewed as a chain of stiff links, or arms. Referring to FIGS. 2A, 3, and 6, a robot is shown having varying numbers of axes. FIG. 2A show a robot 20 having a base 22, a first arm 23 connected to the base 22 and rotatable about a first axis A1, and a second arm 24 connected to the first arm 23 and being rotatable about a second axis A2, a third link 25 connected to the second arm and being rotatable about a third axis A3. This robot 20 is commonly referred to as a three-axis robot. The robot 20 in FIGS. 3 and 6 has the base 22, the first arm or link 23 connected to the base 22 that is rotatable about a first axis A1, the second arm or link 24 connected to the first arm 23 that is rotatable about a second axis A2, the third arm or link 25 connected to the second arm 24 that is rotatable about a third axis A3, a fourth arm or link 26 connected to the third arm 25 rotatable about a fourth axis A4, a fifth arm or link 27 connected to the fourth arm 26 rotatable about a fifth axis A5, a sixth arm or link 28 connected to the fifth arm 27 rotatable about a sixth axis A6 and a tool 29 solidly attached to the sixth arm 28.
In general, two links are joined to each other such that they are rotatable in relation to each other around a rotary axis. Each rotary axis is provided with a position transducer which measures the angle of rotation θi, where “i” stands for the axis number. The configuration of the robot is described by the angles of rotation of the axes. Normally, an industrial robot has five or six rotary axes, but may have as few as three axes, as described above. The final link in the chain includes the tool which, depending on the field of application, may be, a gripper, a glue gun, a sprayer, spot welding equipment, and the like. The position of the tool is defined by a tool center point (TCP). The axes of the robot may also be referred to as joints between the links.
Motion of the robot is created by providing a drive signal to a motor coupled to the joint to effect movement of the robot about the axis of rotation. Many applications require the control of the tool on specific trajectories in Cartesian space. One of the most common trajectories of the tool is a path in a straight line. This path is indicated as 30 in FIGS. 2A, 3, and 6. In order to move the robot tool along the path 30, the line is divided into a large number of positions and the robot joint angles are calculated at each position which specifies the location and orientation requirement on the path. Thus, by commanding the joint positions of the robot at a large number of points on the straight line, the desired motion of the robot tool is generated.
Presently, there are two methods of implementing a desired motion: a programmed motion method or a continuous controlled motion method from a teach pendant, i.e., a linear jog method, in which the destination position is continuously updated “on the fly” or in real-time to control the trajectory. In the programmed motion method, the trajectory of the tool is specified within the program. In the continuous controlled motion (jogging) method, the trajectory is generated in real-time.
The process of transforming a position in Cartesian space to joint angles is commonly called the inverse kinematics. The converse is the forward kinematics which, given a set of joint angles, defines a unique tool position. The forward and inverse kinematics are the key transformations in processing robot motions through computer control.
The inverse kinematics for the robot is not usually unique. For a given location and orientation of the tool in Cartesian space, there is more than one way of configuring the robot arms to place the tool at the desired position. The forward kinematics is always unique, since for a given set of joint angles, the tool can only be at one location and orientation in space.
A common articulated robot geometry has six degrees of freedom. The joint angles starting from the base of the robot are commonly termed the base rotation A1, shoulder rotation A2, elbow rotation A3, wrist roll A4, wrist pitch A5, and wrist yaw A6. The complexity of determining the six joint angles of the six degrees of freedom for a given location component and orientation component directly depends on the robot geometry. Some robot geometries yield a closed form inverse kinematics. In a closed form inverse kinematics, the joint angles are calculated by solving simultaneous equations, which do not require iterations or checks for convergence. Alternatively, some robot geometries do not yield a closed form solution, and iterative methods are generally employed to solve the inverse kinematics by iterating until the solution converges. In either case, when a singular configuration is encountered, the inverse kinematics solution typically degenerates into an infinite number of joint solutions. Consequently, when controlling a robot along a Cartesian path that passes through a singularity, a finite Cartesian speed typically demands an infinite joint speed. This is a longstanding issue in robotics, that is, when the robot approaches a singularity, considerable reconfigurations of certain joint axes are required and these axes also attain very high axis speeds. These reconfigurations may cause the robot to deteriorate over time. Also, any supply lines (electrical, paint, etc.) connected to the tool may tangle or even break as the reconfigurations occur.
For the robot to be able to follow a rectilinear path which passes through a singular point, two problems have to be solved. For one thing, the robot must detect that it approaches a singularity and it must be guided through the singularity such that the rectilinear path is followed. For the reasons described above, the passage through the singularity must not entail the considerable reconfigurations of the axes.
One method of detecting that the robot approaches a singularity, and of passing through the singularity without unnecessary reconfigurations of certain of the robot axes is illustrated in U.S. Pat. No. 5,590,034. The method comprises factorizing the Jacobian matrix of the robot, which is converting the matrix into a product of several factors, wherein each of the factors may be inverted in a simple manner. The Jacobian matrix is factorized in such a way that one of the factors becomes a diagonal matrix. A diagonal matrix is inverted by inverting the diagonal elements. If any of the diagonal elements is smaller than a preset value, this is an indication that the robot is approaching a singularity. On the basis of the diagonal elements which are smaller than this value, it is determined which axes run the risk of attaining high axis velocities and these axes are locked. When the axes are locked this means that the robot movement can only take place in certain directions. A major disadvantage of this solution is that the robot cannot follow a given path through the singularity since the tool does not retain its position through the singularity. Furthermore, the method requires processors with large capacity to factor the matrices and has to deal with numerical stability of the algorithm involved.
U.S. Pat. No. 4,680,519 discloses another method of passing through singularity along a Cartesian path without unnecessary major reconfigurations of certain robot axes. The main object of this patent is to present an iterative method of solving the inverse kinematics of robots with offset wrist where closed form solution does not exist. However, this iterative method breaks down at or near robot singularities. Hence as a side objective, this patent present a different method of solving the inverse kinematics when the robot is at/near singularity, which requires that the wrist joint angles joint angles 4, 5 and 6) be given along with the Cartesian location as input. In moving through the singularity region, for each interpolation point, it needs to know the wrist joint angles and the Cartesian location. The resultant smooth robot motion is achieved in the expense of giving up the Cartesian orientation control along the path as it moves through the singularity region. Furthermore, this patent addresses only the wrist singularity and does not address other singularities.
One common approach to identify the singularities of a given robot structure, is to compute its Jacobian matrix as described below. The conditions that cause the determinant of this Jacobian matrix to be zero are the singularity conditions for this robot.
The Jacobian matrix relates the joint speed to the Cartesian speed of the robot       [                                        v            _                                                            ω            _                                ]    =      J    ⁢                   ⁢                  θ        .            _      
Hence for a given Cartesian path with a certain program Cartesian speeds, its joint speeds are given by             θ      .        _    =            J              -        1              ⁡          [                                                  v              _                                                                          ω              _                                          ]      
So when the determinant of the Jacobian matrix is equal to 0, i.e. det(J)=0, the inverse of J does not exists and a singularity is reached. At such conditions, the joint speed for certain axes are infinite.
For purpose of describing the most common types of singularities encountered in industrial robots, let us consider a 6 axes robot with an inline wrist, as shown in FIG. 1., where the wrist axes (aces 4, 5 and 6) intersect at point called the wrist center point (WCP). For the purpose of exposing the singularity conditions of this robot, the Jacobian matrix of this robot for relating frame 6 velocities to the joint speeds, expressed in frame 3 coordinate frame, can be derived as.                    3        ⁢    J    =      [                            0                                      l            12                                                l            13                                    0                          0                          0                                                  l            21                                    0                          0                          0                          0                          0                                      0                                      l            32                                                l            33                                    0                          0                          0                                                  -                          s              3                                                0                          0                          1                          0                                      c            5                                                0                          0                          1                          0                                      c            4                                                              s              4                        ⁢                          s              5                                                                        c            3                                    0                          0                          0                                      s            4                                                              -                              c                4                                      ⁢                          s              5                                            ]  wherel21=d1+d2s2+d31c3+d32s3l1232 d2c2c3+d2s2s3l3232 d2c2s3−d2s2c3l13=c3(−d31s3+d32c3)+s3(d31c3+d32s3)l33=s3(−d31s3+d32c3)−c3(d31c3+d32s3)Denoting                    3        ⁢    J    =      [                            A                          0                                      B                          C                      ]                  det(J)=det(3J)=det(A)·det(C) and        det(A)=l21(l13l32−l12l33)        det(C)=−s5         where det( ) indicates the determinant of a matrix        
Note that J is singular when the determinant of matrix A is equal to zero and the determinant of matrix C is equal to zero. All together, there are three distinct conditions in which J is singular giving rise to three distinct singularities. A first type of singularity occurs in the wrist when det(C)=0, that is when sin(θ5)=0, where the fourth axis A4 and the sixth axis A6 are aligned, as shown in FIGS. 6 and 7. This is by far the most common type of singularity encountered in the industry. This is commonly referred to as a wrist singularity. A second type of singularity occurs when the distance of a wrist center point (WCP) from the first axis A1 is zero, that is when l21=0, causing det(A) to be 0. In other words, when the WCP is on the line passing through the first axis A1. This type of singularity occurs only when the first axis A1, the second axis A2, and the third axis A3 have sufficient motion ranges, as shown in FIGS. 3 and 4. This is commonly referred to as an overhead singularity. A third type of singularity occurs when the second axis A2 and the third axis A3, are inline, that is when (l13l32−l12l33)=0, causing det(A) to be 0. This type of singularity occurs when the angle between the link axes is 180 degrees, then the second axis A2 and the third axis A3 are fully stretched out, or when the angle between the link axes is zero degree, as shown in FIGS. 2A and 2B. This is commonly referred to as an inline singularity.
For a given Cartesian location and orientation, there are in general eight possible sets of joint angles to reach the position. These correspond to three possible independent configuration sets: a first configuration set having an up configuration and a down configuration, which are related to the second axis A2 and the third axis A3, a second configuration set having a flip configuration and a no-flip configuration, which are related to the wrist axes A4, A5, A6 and a third configuration set having a front configuration and a back configuration, which are related to the sign of the distance of WCP to the first axis A1. A boundary condition of each configuration set represents one type of singularity for the robot. This is another way of identifying the robot singularities besides using the Jacobian matrix, as described above. To put it another way, the singularity occurs when the arms are in a position where the configuration cannot be determined. Hence the up/down configuration set is associated with the inline singularity, the flip/no-flip configuration set is associated with the wrist singularity and finally the front/back configuration set is associated with the overhead singularity.
For robots with an inline wrist, flip/no-flip configurations involve only the wrist axes A4, A5, A6. That is, angles of rotation (θ1,θ2,θ3) about axis A1, A2, and A3 remains the same. If the flip configuration solution for the wrist axes is (θ4,θ5,θ6), then the no-flip solution is given by (θ4±180,−θ5,θ6∓180), and vice versa.
Hence the flip/no-flip configuration can be identified by the sign of θ5. When θ5 is positive, it is in the flip configuration. When θ5 is negative, it is in the no-flip configuration.
At the boundary point of flip and no-flip configuration, i.e. θ5=0, this is the singularity condition. This is by far the most common singularity. It is present in all five and six axes robots and for both inline and offset wrist.
Consider a straight line path that passes through wrist singularity, i.e., with θ5=0 which is the boundary between flip and no-flip configurations, as shown in FIG. 6. As shown in FIG. 7, by maintaining the same “no-flip” configuration along this path, axis A4 has to move 180 degrees in one direction and axis A6 has to move 180 degrees in the opposite direction in one interpolation period just before and after the singular point. Therefore, the fourth axis A4 and sixth axis A6 has to maintain speeds of 18,000 deg/sec (for 10 msec interpolation time) in one interpolation period. Hence, the TCP will have to slowdown, because both axes A4 and A6 are limited by their respective joint speed limits as the robot moves through the singular point. Referring to FIG. 8, the TCP speed slow down is graphically represented. The TCP speed was traveling at a slow speed of 500 mm/s before and after the singularity and drops to less than 100 mm/s during the period where the fourth axis A4 and the sixth axis A6 are limiting to their respective joint speed limits when the robot passes through the wrist singularity.
For a given Cartesian location and orientation, there are two solutions corresponding to the robot's up/down configuration. With reference to FIG. 2C, a line from the start of the second link to the wrist center point (WCP), there are two ways to position the wrist center point (WCP) to reach the desired location. In the up configuration, the elbow is above the line, whereas in the down configuration, the elbow is below the line. Note that the up/down configuration is when the second link and the third link moves in a vertical plane and is referred to as a left/right configuration when these links moves in a horizontal plane as depicted in FIGS. 2A and 2B.
Without loss of generality, the effect of this Inline singularity can be explained with reference to FIGS. 2A and 2B for the horizontal case. The singularity occurs in this type of robot when the first link and the second link are inline, i.e. fully stretched out or fully folded in. Referring to FIG. 2A, if the path passes through the exact singularity, i.e. the boundary point between “right” and “left where the configuration is indeterminable. While moving along this path 30, the first link has to move 180 degrees as it moves through the singular point in order to remain at “right” configuration for the rest of the motion. For an interpolation time of 10 msec, this corresponds to requiring the first link to move at a very high speed of 18,000 deg/sec. By limiting the first link to its joint speed limit, it takes a longer amount of time to pass through this short distance across the singular point, thus slowing down the robot's TCP speed.
Similarly, for a given Cartesian location and orientation there are two solutions corresponding to the robot's front/back configuration. Referring to FIGS. 3 and 4, this involves mainly the first link and indirectly the second link and the third link. If a coordinate frame were attached to the first link, as shown in FIG. 4, the distance of the WCP in the positive x direction is in the front configuration. For the same WCP location, the back configuration is obtained by moving θ1 180 degrees from its current position, and bending θ2,θ3, backwards to reach the same WCP location.
The boundary of front and back configuration is when the distance of WCP to the first axis A1 is zero. At this point, the sign of the distance is neither positive nor negative, which results in a singularity. The first link has to move 180 degrees in one interpolation period, which means a first link joint speed of 18,000 deg/sec (for 10 msec interpolation period). Again TCP speed will be slowed to limit the first link's joint speed within its limit, as is shown in FIG. 5. The TCP speed before and after the singularity is 500 mm/s, but slows to about 100 mm/s due to this joint speed limit.
Accordingly, the related art methods are characterized by one or more inadequacies as described above. Specifically, the related art methods are unable to maintain the TCP speed through the singularity and are unable to maintain the TCP on the path through all the common types of singularities. The related art methods also do not suggest minimizing any errors between the location and orientation components while moving along the path that passes through or near the singularity. Furthermore, none of the related art methods change configuration of the robot arms to move through the singularity.